
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <laser_geometry/laser_geometry.hpp>
#include "aeb/node.h"
rclcpp::Node::SharedPtr node;

class ObstacleNode {
public:
  using PointT = pcl::PointXYZI;

  ObstacleNode() {
    node->declare_parameter<std::string>("sub_scan_topic", "/scan_bottom");
    sub_scan_topic = node->get_parameter("sub_scan_topic").as_string();
    scan_cloud_pub = node->create_publisher<sensor_msgs::msg::PointCloud2>("/obstacle_cloud", 5);
    scan_sub = node->create_subscription<sensor_msgs::msg::LaserScan>(sub_scan_topic, 1, std::bind(&ObstacleNode::scan_callback, this, std::placeholders::_1));
  }

  void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan){
    sensor_msgs::msg::PointCloud2 cloud;
    projector_.projectLaser(*scan,cloud);
    scan_cloud_pub->publish(cloud);
  }

private:
  // ros::NodeHandle private_nh;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scan_cloud_pub;
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub;
  laser_geometry::LaserProjection projector_;

  std::string sub_scan_topic;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  node =  std::make_shared<rclcpp::Node>("obstacle_node");

  RCLCPP_INFO_STREAM(node->get_logger(),"Start Obstacle Node");
  ObstacleNode obstacleNode;
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

